/*!
 * \file cmd_queue.c
 * \brief 串口指令队列实现（支持多队列、多帧头、可配置帧长）
 * \version 1.3
 * \date 2024.10
 * \copyright wo4fisher
 */

#include "cmd_queue.h"

// 平台相关：关中断保护
#define ENTER_CRITICAL()
#define LEAVE_CRITICAL()

// 初始化队列
void queue_init(QueueHandle *q) { q->_head = q->_tail = 0; }

// 清空队列
void queue_reset(QueueHandle *q) { q->_head = q->_tail = 0; }

// 入队操作
void queue_push(QueueHandle *q, qdata _data) {

  qsize next_head = (q->_head + 1) % QUEUE_MAX_SIZE;
  if (next_head != q->_tail) {
    q->_data[q->_head] = _data;
    q->_head = next_head;
  }
}

// 出队操作
uint8_t queue_pop(QueueHandle *q, qdata *_data) {
  uint8_t success = 0;

  if (q->_tail != q->_head) {
    *_data = q->_data[q->_tail];
    q->_tail = (q->_tail + 1) % QUEUE_MAX_SIZE;
    success = 1;
  }

  return 1;
}

// 获取队列大小
qsize queue_size(QueueHandle *q) {
  qsize size;

  size = (q->_head + QUEUE_MAX_SIZE - q->_tail) % QUEUE_MAX_SIZE;

  return size;
}

// 提取完整指令帧（支持多帧头、帧长可配）
qsize queue_find_cmd_ext(QueueHandle *q, qdata *buffer, qsize buf_len,
                         const qdata *frame_header, uint8_t header_len,
                         uint8_t total_frame_len) {
  qdata _data;
  uint8_t rxSum = 0;
  uint8_t received_pos = 0;

  if (buffer == NULL || buf_len < total_frame_len || frame_header == NULL ||
      header_len == 0 || total_frame_len <= header_len) {
    return 0;
  }
  // 成功：连续读取一帧数据
  // 错误：直接退出，直到下一次再调用次函数
  while (queue_size(q) > 0) {
    // 没有数据，退出
    if (!queue_pop(q, &_data))
      break;

    if (received_pos < header_len) { // 检测帧头
      if (_data != frame_header[received_pos]) {
        // 检测帧头失败，received_pos归零，从头来
        received_pos = 0;
        continue;
      } else {
        buffer[received_pos++] = _data;
      }
    } else { // 帧数据缓存
      buffer[received_pos++] = _data;
    }

    // if (received_pos >= total_frame_len) {
    //   for (int i = 0; i < total_frame_len; i++) {
    //     rxSum += buffer[i];
    //   }

    //   if ((rxSum & 0xFF) == 0) {
    //     return total_frame_len;
    //   } else {
    //     received_pos = 0;
    //   }
    // }
    if (received_pos >= total_frame_len) {
      return total_frame_len;
    }
  }
  return 0;
}
